#!/usr/bin/python
# coding:utf-8

# os basic
import threading, time, os, traceback, gc, stat

# robot core
import jkzuc

# socket
import socket, select

# data structure
import json, configparser, zlib, string, collections, struct

# jaka import
import tool_function
import tcp_command_process
import pylog

c = jkzuc.command()
s = jkzuc.stat()
e = jkzuc.error()
tcp_logger = pylog.tcp_logger


class MiddlewareException(Exception):

    def __init__(self, reason=""):
        super(Exception, self).__init__(reason)
        self.reason = reason


def update_jkzuc_status():
    try:
        s.poll()
    except Exception:
        # tcp_logger().error("s.poll :"+ traceback.format_exc())

        reason = "jkzuc.stat poll err,perhaps robot controller dead"
        raise MiddlewareException(reason)


filename = "/usr/etc/jkzuc/configs/JAKA/optionalInfoConfig.ini"
section_attr = "ATTROPTIONALDATA"
section_stat = "STATOPTIONALDATA"
section_port = "PORTCONFIG"


def save_decimal(pos):
    for i in range(0, 6):
        pos[i] = float(format(pos[i], "0.5f"))
    return pos


def check_optionalInfoConfig():
    maps_ATTROPTIONALDATA = (
        "joint_actual_position",
        "joint_position",
        "actual_position",
        "din",
        "dout",
        "ain",
        "aout",
        "tio_din",
        "tio_dout",
        "tio_ain",
        "mb_slave_din",
        "mb_slave_dout",
        "mb_slave_ain",
        "mb_slave_aout",
        "pn_dev_din",
        "pn_dev_dout",
        "pn_dev_ain",
        "pn_dev_aout",
        "eip_adpt_din",
        "eip_adpt_dout",
        "eip_adpt_ain",
        "eip_adpt_aout",
        "task_state",
        "homed",
        "task_mode",
        "interp_state",
        "enabled",
        "paused",
        "rapidrate",
        "current_tool_id",
        "current_user_id",
        "on_soft_limit",
        "emergency_stop",
        "drag_near_limit",
        "funcdi",
        "powered_on",
        "inpos",
        "motion_mode",
        "curr_tcp_trans_vel",
        "protective_stop",
        "tio_key",
    )
    maps_STATOPTIONALDATA = ("drag_status", "num", "setup", "pinmap", "mode", "status", "extio", "errcode", "errmsg", "monitor_data", "torqsensor")

    global filename
    if not os.path.exists(filename):
        try:
            os.mknod(filename)
            os.chmod(filename, stat.S_IRWXU)
        except Exception:
            tcp_logger().warn("fail to generate optionalInfoConfig.ini")

    config = configparser.ConfigParser()
    config.read(filename)
    sections = config.sections()
    configChange = False

    if section_attr not in sections:
        configChange = True
        config.add_section(section_attr)
    keys = config.options(section_attr)
    for k in maps_ATTROPTIONALDATA:
        if k not in keys:
            configChange = True
            config.set(section_attr, k, str(0))

    if section_stat not in sections:
        configChange = True
        config.add_section(section_stat)
    keys = config.options(section_attr)
    for k in maps_STATOPTIONALDATA:
        if k not in keys:
            configChange = True
            config.set(section_stat, k, str(0))

    if section_port not in sections:
        configChange = True
        config.add_section(section_port)
    keys = config.options(section_port)
    if "port10000_delay_ms" not in keys:
        configChange = True
        config.set(section_port, "port10000_delay_ms", str(200))

    if configChange:
        with open(filename, "w+") as fd:
            config.write(fd)

    for item in config.options(section_attr):
        if config.getint(section_attr, item) == 1:
            tcp_command_process.maps_attr_used.append(item)
    for item in config.options(section_stat):
        if config.getint(section_stat, item) == 1:
            tcp_command_process.maps_stat_used.append(item)
    tcp_command_process.port10000_delay_ms = config.getint(section_port, "port10000_delay_ms")

    del config


def get_data(port=10000):
    port_stat = port == 10000
    port_compressStat = port == 10004
    err_check_bit = 0x02
    if port_stat:
        err_check_bit = 0x02
    elif port_compressStat:
        err_check_bit = 0x04

    ret_dict = collections.OrderedDict()
    ret_dict["len"] = 0
    try:
        update_jkzuc_status()
    except MiddlewareException as err:
        tcp_logger().warn("fail to get data")
        errCode = 65535
        errString = err.reason
        ret_dict["errcode"] = hex(errCode)
        ret_dict["errmsg"] = errString
        jason_data = json.dumps(ret_dict)
        return jason_data
    # var_list = []
    error_stat = e.poll()
    if error_stat:
        kind, last_errcode, last_errstring = error_stat
        if last_errcode != 0:
            repeatflag = (~(err_check_bit)) & 0xFF
            # print ("before set_grpc_errcode")
            c.set_grpc_errcode(int(kind), int(last_errcode), str(last_errstring), repeatflag, 0)
    else:
        kind, last_errcode, last_errstring, repeatflag = s.grpc_errcode
        # print str(kind),str(last_errcode),str(last_errstring),str(repeatflag)
        if (
            last_errcode != 0 and repeatflag & err_check_bit
        ):  # jungle the 2nd bit of repeatflag if 1 (10000port), jungle the 3rd bit of repeatflag if 1 (10004port)
            repeatflag = repeatflag & (~(err_check_bit)) & 0xFF  # the 2nd bit for TCP10000 repeat, #the 3rd bit for TCP10000 repeat
            c.set_grpc_errcode(int(kind), int(last_errcode), str(last_errstring), repeatflag, 0)
        else:
            kind = 0
            last_errcode = 0
            last_errstring = ""
    if kind in (jkzuc.NML_ERROR, jkzuc.OPERATOR_ERROR):
        tcp_logger().warn("get sdk data failed -" + str(s.grpc_errcode))
        errCode = int(last_errcode)
        errString = last_errstring
    else:
        errCode = 0
        errString = ""
    # TODO
    # #user_variable = jaka_pb2.UserVariable()
    # filename = '/usr/etc/jkzuc/configs/JAKA/jaka_user.var'
    # with open(filename, 'r+') as fd:
    #     for line in fd.readlines():
    #         line = line.rstrip()
    #         item_list = line.split()
    #         item_dict = {}
    #         item_dict["id"] = int(item_list[0])
    #         item_dict["value"] = float(item_list[1])
    #         item_dict["alias"] = str(item_list[2])
    #         var_list.append(item_dict)
    # print ("get robot data")

    # check_optionalInfoConfig()
    global filename
    config = configparser.ConfigParser()
    config.read(filename)

    extio = {}
    try:
        if "drag_status" in tcp_command_process.maps_stat_used:
            ret_dict["drag_status"] = s.motion_mode == jkzuc.TRAJ_MODE_DRAG

        if "num" in tcp_command_process.maps_stat_used:
            extio["num"] = s.extio_num
        if "setup" in tcp_command_process.maps_stat_used:
            extio["setup"] = s.extio_setup
        if "pinmap" in tcp_command_process.maps_stat_used:
            extio["pinmap"] = s.extio_pinmap
        if "mode" in tcp_command_process.maps_stat_used:
            extio["mode"] = s.extio_mode
        if "status" in tcp_command_process.maps_stat_used:
            extio["status"] = s.extio_stat
        if "extio" in tcp_command_process.maps_stat_used:
            ret_dict["extio"] = extio
        if "errcode" in tcp_command_process.maps_stat_used:
            ret_dict["errcode"] = hex(errCode)
        if "errmsg" in tcp_command_process.maps_stat_used:
            ret_dict["errmsg"] = errString
        if "monitor_data" in tcp_command_process.maps_stat_used:
            ret_dict["monitor_data"] = s.monitor_data
        if "torqsensor" in tcp_command_process.maps_stat_used:
            ret_dict["torqsensor"] = s.torqsensor

        ret_dict["instVel"] = []
        for i in range(0, 6):
            ret_dict["instVel"] += [s.joint[i]["velocity"]]

        for k in tcp_command_process.maps_attr_used:
            if k == "actual_position":
                tcp_pos = list((getattr(s, "actual_position"))[0:6])
                ret_dict[k] = save_decimal(tcp_pos)
            if k == "position":
                tcp_pos = list((getattr(s, "position"))[0:6])
                ret_dict[k] = save_decimal(tcp_pos)
            elif k == "joint_actual_position":
                joint_pos = list((getattr(s, "joint_position"))[0:6])
                ret_dict[k] = save_decimal(joint_pos)
            elif k == "joint_position":
                joint_pos = list((getattr(s, "joint_position"))[0:6])
                ret_dict[k] = save_decimal(joint_pos)
            elif k == "tool_position":
                tool_pos = list((getattr(s, "tool_position"))[0:6])
                ret_dict[k] = save_decimal(tool_pos)
            elif k == "point_key":
                ret_dict[k] = s.tio_uikey[1]
            elif k == "tio_key":
                ret_dict[k] = s.tio_uikey[0:3]
            else:
                ret_dict[k] = getattr(s, k)

        ret_dict["netState"] = 1
    except:
        tcp_logger().warn("fail to get key from optionalInfoConfig.ini")

    json_data = json.dumps(ret_dict)
    while ret_dict["len"] != json_data.__len__():
        ret_dict["len"] = json_data.__len__()
        json_data = json.dumps(ret_dict)

    del config
    del extio
    del ret_dict
    return json_data


class SdkCompressStatusServer(threading.Thread):

    class NetExceptionHandle:

        def __init__(self):
            self.lastTime = 0
            self.isMoving = False

    def __init__(self, threadname, port):
        threading.Thread.__init__(self, name=threadname)
        self.port = port
        self.netExceHand = self.NetExceptionHandle()

    def netHandle(self):
        update_jkzuc_status()
        rs = s.net_exception_handle
        mot_type = rs[0]
        time_limit = rs[1]
        if mot_type != 0:
            if self.netExceHand.isMoving:
                if time.time() - self.netExceHand.lastTime > time_limit:
                    self.netExceHand.isMoving = False
                    if mot_type == 1:
                        cmd = {"cmdName": "pause_program"}
                        cmd = json.dumps(cmd)
                        tcp_command_process.process_tcp_command(cmd)
                    if mot_type == 2:
                        cmd = {"cmdName": "stop_program"}
                        cmd = json.dumps(cmd)
                        tcp_command_process.process_tcp_command(cmd)

    def request10004PortData(self, client_sock, recvData, inputs):
        try:
            # FIXME: why here is not json
            if string.find(recvData.decode(), "data_request") != -1:
                data = zlib.compress(get_data(self.port))
                client_sock.sendall(data)
                del data
            self.netExceHand.isMoving = True
            self.netExceHand.lastTime = time.time()
        except Exception:  # as e:
            tcp_logger().warn("[TCP10004] {}.".format(traceback.format_exc()))
            inputs.remove(inputs[1])

    def run(self):
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(("0.0.0.0", self.port))  # 10004
        server.setblocking(0)
        server.listen(2)
        inputs = [
            server,
        ]
        tcp_logger().info("[TCP10004] server is running!" + str(self.port))

        while True:
            readable_list, writeable_list, error_list = select.select(inputs, [], [], 0.1)

            # network exception handle
            try:
                self.netHandle()
            except MiddlewareException:
                pass

            if len(readable_list) > 0:
                for r in readable_list:
                    if server == r:
                        inputs = inputs[:1]
                        request, address = r.accept()
                        tcp_logger().info("[TCP10004] a new connect from : {}  ".format(str(address)))
                        request.setblocking(0)
                        inputs.append(request)
                    else:
                        try:
                            received = r.recv(1024)
                            if received:
                                self.request10004PortData(inputs[1], received, inputs)
                            else:
                                inputs.remove(r)
                                tcp_logger().info("[TCP10004] a client exit : {}  ".format(str(address)))
                        except Exception:
                            tcp_logger().warn("[TCP10004] {}".format(traceback.format_exc()))
            else:
                time.sleep(1)


class SdkStatusServer(threading.Thread):

    def __init__(self, threadname, port):
        threading.Thread.__init__(self, name=threadname)
        self.port = port

    def run(self):
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(("0.0.0.0", self.port))
        server.setblocking(False)
        server.listen(10)
        clientlist = []
        tcp_logger().info("[TCP10000] raw status server is running!" + str(self.port))
        while True:
            try:
                client, addr = server.accept()
            except:
                pass
            else:
                # print("10000 a new client: %s" % str(addr))
                tcp_logger().info("[TCP10000] a new connect from : {}  ".format(str(addr)))
                client.setblocking(False)
                clientlist.append((client, addr))
            data = None
            if len(clientlist):
                data = get_data(self.port)
            for cSocket, cAddr in clientlist:
                try:
                    ret = cSocket.sendall(data)
                except Exception:
                    cSocket.close()
                    clientlist.remove((cSocket, cAddr))
                    # print("a client exit: %s" % str(cAddr))
                    tcp_logger().info("[TCP10000] a connect exit from : {}  ".format(str(cAddr)))
                    pass
                else:
                    if ret == None:
                        pass
                    else:
                        cSocket.close()
                        clientlist.remove((cSocket, cAddr))
                        tcp_logger().info("[TCP10000] %s exit" % (str(cAddr)))
            del data

            time.sleep(tcp_command_process.port10000_delay_ms / 1000)


class OtaServer(threading.Thread):

    def __init__(self, threadname, port):
        threading.Thread.__init__(self, name=threadname)
        self.port = port
        self.maps_ota = (
            "joint_actual_position",
            "actual_position",
            "task_state",
            "task_mode",
            "interp_state",
            "enabled",
            "paused",
            "rapidrate",
            "protective_stop",
            "emergency_stop",
            "powered_on",
            "inpos",
            "cab_run_time",
            "cab_accu_run_time",
            "payload",
            "installation_angle",
        )

    def get_ota_data(self, ota_data):
        try:
            update_jkzuc_status()
            ret_dict = {}
            insAveragePower = {}
            instTemperature = {}
            instVoltage = {}
            currentFluctuation = {}
            sumRunningCircles = {}
            # sumRunningTime = {}
            curRunningCircles = {}
            # curRunningTime = {}
            positionDiff = {}
            dualEncoderDiff = {}
            jointDcVol = {}
            jointTorque = {}
            instCurrent = {}
            jointVel = {}
            senddata = {}
            kind, last_errcode, last_errstring, repeatflag = s.grpc_errcode
            # print str(kind),str(last_errcode),str(last_errstring),str(repeatflag)
            if last_errcode != 0 and repeatflag & 0x08:  # jungle the 4th bit of repeatflag if 1
                repeatflag = repeatflag & (~(0x08)) & 0xFF  # the 4th bit for TCP10003 repeat
                c.set_grpc_errcode(int(kind), int(last_errcode), str(last_errstring), repeatflag, 0)
            else:
                kind = 0
                last_errcode = 0
                last_errstring = ""
            # errtype = int(kind)
            errCode = int(last_errcode)
            # errString = last_errstring

            for k in self.maps_ota:
                if k == "actual_position":
                    ret_dict[k] = (getattr(s, "position"))[0:6]
                elif k == "joint_actual_position":
                    ret_dict[k] = (getattr(s, "joint_position"))[0 : s.robot_joint_num]
                elif k == "tool_position":
                    ret_dict[k] = (getattr(s, "tool_position"))[0:6]
                elif k == "cab_run_time":
                    ret_dict[k] = s.monitor_data[7]
                elif k == "cab_accu_run_time":
                    ret_dict[k] = s.monitor_data[8]
                else:
                    ret_dict[k] = getattr(s, k)

            ret_dict["drag_status"] = s.motion_mode == jkzuc.TRAJ_MODE_DRAG
            ret_dict["errcode"] = hex(errCode)
            monitor_data = s.monitor_data
            servo_data = s.servo_data
            for i in range(0, s.robot_joint_num):
                instCurrent["joint_" + str(i + 1)] = monitor_data[5][i][0]
                instVoltage["joint_" + str(i + 1)] = monitor_data[5][i][1]
                instTemperature["joint_" + str(i + 1)] = monitor_data[5][i][2]
                insAveragePower["joint_" + str(i + 1)] = monitor_data[5][i][3]
                currentFluctuation["joint_" + str(i + 1)] = monitor_data[5][i][4]
                sumRunningCircles["joint_" + str(i + 1)] = monitor_data[5][i][5]
                curRunningCircles["joint_" + str(i + 1)] = monitor_data[5][i][7]
                jointTorque["joint_" + str(i + 1)] = monitor_data[5][i][9]
                positionDiff["joint_" + str(i + 1)] = servo_data[i][0]
                dualEncoderDiff["joint_" + str(i + 1)] = servo_data[i][1]
                jointDcVol["joint_" + str(i + 1)] = servo_data[i][2]
                jointVel["joint_" + str(i + 1)] = s.joint[i]["velocity"]

            ret_dict["insAveragePower"] = insAveragePower
            ret_dict["currentFluctuation"] = currentFluctuation
            ret_dict["sumRunningCircles"] = sumRunningCircles
            ret_dict["sumRunningTime"] = monitor_data[5][5][6]
            ret_dict["curRunningCircles"] = curRunningCircles
            ret_dict["curRunningTime"] = monitor_data[5][5][8]
            ret_dict["instVoltage"] = instVoltage
            ret_dict["instTemperature"] = instTemperature
            ret_dict["positionDiff"] = positionDiff
            ret_dict["dualEncoderDiff"] = dualEncoderDiff
            ret_dict["jointDcVol"] = jointDcVol
            ret_dict["jointTorque"] = jointTorque
            ret_dict["instCurrent"] = instCurrent
            ret_dict["jointVel"] = jointVel
            ret_dict["cabType"] = s.cab_type
            ret_dict["cabTemperature"] = s.monitor_data[2]
            ret_dict["robotAveragePower"] = s.monitor_data[3]
            ret_dict["scb_version"] = (getattr(s, "monitor_data"))[0:2]
            ret_dict["controller_version"] = jkzuc.controller_version
            ret_dict["servoVersion"] = s.servo_version
            ret_dict["programName"] = s.file

            senddata["data"] = ret_dict
            senddata["rtc"] = int(round(time.time() * 1000))
            tool_function_ctx = tool_function.ModuleCtx(tcp_logger)
            senddata["robotID"] = str(tool_function.read_robot_info(tool_function_ctx)[0])
            senddata["controlCabinetID"] = str(s.monitor_data[6])

            # package header
            ver = 1
            body = json.dumps(senddata)
            header = [ver, body.__len__()]
            # headPack =struct.pack("!2I", *header)
            # unpack =  struct.unpack("!2I",headPack)
            # print ("upack:",unpack)
            ota_data.append(struct.pack("!2I", *header) + body.encode())

            del body
            del ret_dict
            del senddata
            del insAveragePower
            del instTemperature
            del instVoltage
            del currentFluctuation
            del sumRunningCircles
            del curRunningCircles
            del dualEncoderDiff
            del jointDcVol
            del positionDiff
            del jointTorque
            del instCurrent
            del jointVel
            gc.collect()
        except Exception:
            tcp_logger().warn("[OTA] {}".format(traceback.format_exc()))
            ota_data.append("")

    def run(self):
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(("0.0.0.0", self.port))
        server.setblocking(False)
        server.listen(10)
        clientlist = []
        tcp_logger().info("[OTA] server is running! port:" + str(self.port))
        while True:
            try:
                client, addr = server.accept()
            except:
                pass
            else:
                # print ("a new client: %s" % str(addr))
                tcp_logger().info("[OTA] a client exit : {}  ".format(str(addr)))
                client.setblocking(False)
                clientlist.append((client, addr))
            data = []
            if len(clientlist):
                self.get_ota_data(data)
            for cSocket, cAddr in clientlist:
                try:
                    ret = cSocket.sendall(data[0])
                except Exception:
                    cSocket.close()
                    clientlist.remove((cSocket, cAddr))
                    tcp_logger().info("[OTA] a client exit: %s" % str(cAddr))
                    pass
                else:
                    if ret == None:
                        pass
                    else:
                        cSocket.close()
                        clientlist.remove((cSocket, cAddr))
                        tcp_logger().info("[OTA] %s exit" % (str(cAddr)))
            del data
            gc.collect()
            time.sleep(0.08)


class SdkCommandServer(threading.Thread):

    def __init__(self, threadname, port):
        threading.Thread.__init__(self, name=threadname)
        self.port = port

    def process_select(self, client_sock, recvData, inputs):
        # print recvData.decode()
        try:
            sendData = tcp_command_process.process_tcp_command(recvData.decode())
            try:
                client_sock.sendall(sendData.encode())
            except Exception:
                try:
                    client_sock.sendall(sendData)
                except Exception:
                    tcp_logger().warn("[TCP10001] {}".format(traceback.format_exc()))
                    inputs.remove(inputs[1])
        except Exception:
            tcp_logger().warn("[TCP10001] {}".format(traceback.format_exc()))
            inputs.remove(inputs[1])

    def run(self):
        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        server.bind(("0.0.0.0", self.port))
        server.setblocking(0)
        server.listen(2)
        inputs = [
            server,
        ]
        tcp_logger().info("[TCP10001] command server is running! port:" + str(self.port))
        while True:
            try:
                readable_list, writeable_list, error_list = select.select(inputs, [], [])

                for r in readable_list:
                    if server == r:
                        if len(inputs) > 1:
                            inputs.remove(inputs[1])
                        request, address = r.accept()
                        tcp_logger().info("[TCP10001] a new connect from " + str(address))
                        request.setblocking(0)
                        inputs.append(request)
                    else:
                        try:
                            received = r.recv(1024)
                            if received:
                                self.process_select(inputs[1], received, inputs)
                            else:
                                inputs.remove(r)
                                tcp_logger().info("[TCP10001] a client exit" + str(address))
                        except Exception:
                            tcp_logger().warn("[TCP10001] {}".format(traceback.format_exc()))
            except Exception:
                tcp_logger().warn("[TCP10001] {}".format(traceback.format_exc()))


def Tcp_server_start():
    check_optionalInfoConfig()

    # start SDK server TCP10004
    Thread_compress_robot_status = SdkCompressStatusServer("tcp10004", 10004)
    Thread_compress_robot_status.setDaemon(True)
    Thread_compress_robot_status.start()
    # start SDK server port10000
    Thread_robot_status = SdkStatusServer("tcp10000", 10000)
    Thread_robot_status.setDaemon(True)
    Thread_robot_status.start()
    # start SDK server port10003
    Thread_ota_server = OtaServer("tcp10003", 10003)
    Thread_ota_server.setDaemon(True)
    Thread_ota_server.start()
    # start SDK server port10001
    Thread_robot_command = SdkCommandServer("tcp10001", 10001)
    Thread_robot_command.setDaemon(True)
    Thread_robot_command.start()
    while True:
        time.sleep(24 * 60 * 60)


# for testing
if __name__ == "__main__":
    Tcp_server_start()
